#include <graphics/Joint.h>
#include <graphics/Skeleton.h>

namespace ne
{
    const uint16_t Joint::MAX_JOINT_HANDLE = 0xFFFE;
    const uint16_t Joint::JOINT_NO_PARENT_HANDLE = 0xFFFF;

    Joint::Joint(Skeleton *creator, const uint16_t handle, const std::string &name)
        : Node(),
          Object(name),
          mCreator(creator),
          mHandle(handle),
          mBindDerivedInversePosition(Vector3::ZERO),
          mBindDerivedInverseOrientation(Quaternion::IDENTITY),
          mBindDerivedInverseScale(Vector3::ONE),
          mManuallyControlled(false)
    {
    }

    Joint* Joint::createChild(const uint16_t handle,
                              const std::string &name,
                              const Vector3 &position,
                              const Quaternion &orientation,
                              const Vector3 &scale)
    {
        Joint *child = mCreator->createJoint(handle, name);
        if (child)
        {
            child->setPosition(position);
            child->setOrientation(orientation);
            child->setScale(scale);
        }
        // Here maybe has bug !
        if (!addChild(child))
        {
            mCreator->destroyJoint(child);
            child = 0;
        }
        return child;
    }

    bool Joint::destroyChild(Joint *child)
    {
        removeChild(child);
        return mCreator->destroyJoint(child);
    }

    bool Joint::destroyChild(const std::string &name)
    {
        return destroyChild(mCreator->getJoint(name));
    }

    // Here maybe has bug !
    void Joint::destroyAllChildren()
    {
        for (NodeSetIterator it=mChildren.begin(); it!=mChildren.end(); ++it)
        {
            Joint *child = dynamic_cast<Joint*>(*it);
            if (child)
            {
                removeChild(child);
                child->destroyAllChildren();
            }
            mCreator->destroyJoint(child);
        }
    }

    void Joint::setObjectName(const std::string &name)
    {
        if (mCreator->_prepareRenameJoint(this, name))
        {
            Object::setObjectName(name);
        }
    }

    uint16_t Joint::getHandle() const
    {
        return mHandle;
    }

    void Joint::setBinding()
    {
        setInitial();
        mBindDerivedInversePosition = -_getDerivedPosition();
        mBindDerivedInverseOrientation = _getDerivedOrientation().inverse();
        mBindDerivedInverseScale = Vector3::ONE / _getDerivedScale();
    }

    void Joint::setManuallyControlled(const bool manuallyControlled)
    {
        mManuallyControlled = manuallyControlled;
        mCreator->_notifyManualJointChange(this);
    }

    bool Joint::isManuallyControlled() const
    {
        return mManuallyControlled;
    }

    // Used for World Transform
    void Joint::getOffsetTransformMatrix(Matrix4 &m) const
    {
        Quaternion orientation = _getDerivedOrientation() * _getBindDerivedInverseOrientation();
        Vector3 scale = _getDerivedScale() * _getBindDerivedInverseScale();
        Vector3 position = _getDerivedPosition() + orientation * (_getBindDerivedInversePosition() * scale);
        m.makeTransformMatrix(position, orientation, scale);
    }

    void Joint::needUpdate(const bool forceParentUpdate)
    {
        Node::needUpdate(forceParentUpdate);
        if (isManuallyControlled())
        {
            mCreator->_notifyManualJointsDirty();
        }
    }

    Joint::~Joint()
    {
    }

    void Joint::_setHandle(const uint16_t handle)
    {
        if (handle <= MAX_JOINT_HANDLE)
        {
            mHandle = handle;
        }
    }

    const Vector3& Joint::_getBindDerivedInversePosition() const
    {
        return mBindDerivedInversePosition;
    }

    const Quaternion& Joint::_getBindDerivedInverseOrientation() const
    {
        return mBindDerivedInverseOrientation;
    }

    const Vector3& Joint::_getBindDerivedInverseScale() const
    {
        return mBindDerivedInverseScale;
    }
}
